#!/usr/bin/python3
# -*- coding: utf-8 -*-

import time
import socket


ip_mapping = {
    # 左边手指
    51: '169.254.61.230',
    52: '169.254.61.230',
    53: '169.254.61.230',
    54: '169.254.61.230',
    55: '169.254.61.230',
    56: '169.254.61.230',
    # 右边手指
    61: '169.254.61.230',
    62: '169.254.61.230',
    63: '169.254.61.230',
    64: '169.254.61.230',
    65: '169.254.61.230',
    66: '169.254.61.230'
}

port_mapping = {
    # 左边手指
    51: 1030,
    52: 1030,
    53: 1030,
    54: 1030,
    55: 1030,
    56: 1030,
    # 右边手指
    61: 1037,
    62: 1037,
    63: 1037,
    64: 1037,
    65: 1037,
    66: 1037
}

id_mapping = {
    # 左边手指
    51: 1,
    56: 2,
    52: 3,
    53: 4,
    54: 5,
    55: 6,
    # 右边手指.
    61: 1,
    66: 2,
    62: 3,
    63: 4,
    64: 5,
    65: 6
}

'''
51-56为左手手指，61-66为右手手指
51/61:大拇指上下移动，范围上限：1700 ，范围下限：600
56/66:大拇指左右移动，范围上限：1700 ，范围下限：150
52/62:食指，范围上限：1800 ，范围下限：750
53/63:中指，范围上限：1800 ，范围下限：750
54/64:无名指，范围上限：1800 ，范围下限：750
55/65:小拇指，范围上限：1800 ，范围下限：750
'''
arm_max_min_limit = {
    # 左边手指
    51: (1700, 600),
    52: (1950, 750),
    53: (1800, 750),
    54: (1800, 750),
    55: (1800, 750),
    56: (1800, 150),
    # 右边手指
    61: (1700, 600),
    62: (1800, 750),
    63: (1800, 750),
    64: (1800, 750),
    65: (1800, 750),
    66: (1700, 150)
}


# 公共模块部分
def hex2bin(hex_str: str, bin_width: int = -1):
    hex_str = hex_str.strip()
    if hex_str[0] in ['+', '-', '_'] or hex_str[-1] == '_' or '__' in hex_str:
        print('输入不合法，必须为16进制补码，不允许带正负号，首尾不能是下划线，不能连续出现两个下划线')
    elif hex_str[:2].lower() == '0x':
        hex_str = hex_str[2:]
    hex_str = hex_str.replace('_', '')
    bin_str = ''.join([oneHex2fourBin(s) for s in hex_str])

    for i in range(len(bin_str) - 1):
        if bin_str[i + 1] == bin_str[0]:
            if i + 1 == len(bin_str) - 1:
                bin_str = bin_str[i:]
            else:
                continue
        else:
            bin_str = bin_str[i:]
            break
    if bin_str == '00':
        bin_str = '0'
    if bin_width == -1:
        pass
    elif bin_width < len(bin_str):
        print('位宽参数<16进制补码，请修正位宽')
    else:
        bin_str = bin_str[0] * (bin_width - len(bin_str)) + bin_str
    return '0b' + bin_str


def dec2hex(data: int):
    return hex(data % 256)


def bin2dec(bin_str: str):
    bin_str = bin_str.strip()
    if bin_str[:2] == '0b':
        if bin_str[2] == '_':
            bin_str = bin_str[3:]
        else:
            bin_str = bin_str[2:]
    if bin_str[0] == '_':
        print('输入不合法，首字符不能是下划线，且不允许出现连续两个下划线')
    elif bin_str[0] == '0':
        return int(bin_str, base=2)
    elif bin_str[0] == '1':
        a = int(bin_str, base=2)
        bin_str = bin_str.replace('_', '')
        return a - 2 ** len(bin_str)
    else:
        print('输入不合法，必须为2进补码，不允许带正负号')


def oneHex2fourBin(one_hex: str):
    raw = {'A': '1010', 'B': '1011', 'C': '1100', 'D': '1101', 'E': '1110', 'F': '1111'}
    if one_hex in [str(i) for i in range(10)]:
        return '{:0>4}'.format(str(bin(int(one_hex)))[2:])
    elif one_hex.upper() in ['A', 'B', 'C', 'D', 'E', 'F']:
        return raw[one_hex.upper()]
    else:
        print('十六进制格式错误, 只能包含0~9， a~f，A~F')


# 手指电机类
class Finger:

    def __init__(self):
        pass

    def __del__(self):
        pass


    # 电机执行命令
    def arm_send_command(self, ip, port, command):
        try:
            msg = bytearray.fromhex(command)
            # print(msg)
            udp_monitor_addr1 = (ip, port)
            udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

            udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
            # print('命令发送成功...')
        except:
            print('运行命令发送失败...')


    # 手指电机发送记录返回值
    def arm_send_command_rlt(self, ip, port, command):
        try:
            msg = bytearray.fromhex(command)
            # print(msg)
            udp_monitor_addr1 = (ip, port)
            udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

            udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
            # print('命令发送成功...')
            # time.sleep(0.0001) # 求情等待时间
            udp_monitor_sock1.settimeout(0.01) # 请求超时时间，不设置会等待30s超时
            msg, address = udp_monitor_sock1.recvfrom(1024)
            # print(msg)
            # print(type(msg))
            try:
                if msg is not None and len(msg) > 0:
                    result_recv_data = msg.hex()
                    ls = []
                    for i in range(len(result_recv_data) // 2):
                        ls.append(result_recv_data[i * 2] + result_recv_data[i * 2 + 1])

                    if len(ls) == 0:
                        return None

                    # print(ls)
                    # print('--------------------')
                    low = ls[8]
                    high = ls[9]
                    real_controal = bin2dec(hex2bin((low + high)))

                    return real_controal
                else:
                    return None
            except:
                return None
        except:
            print('获取电机命令发送失败...')
            return None


    # 手指电机发送记录返回错误值
    def arm_error_command_rlt(self, ip, port, command):
        rlt = (0, False)
        msg = bytearray.fromhex(command)
        udp_monitor_addr1 = (ip, port)
        udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
        udp_monitor_sock1.settimeout(0.01) # 请求超时时间，不设置会等待30s超时
        msg, address = udp_monitor_sock1.recvfrom(1024)

        if msg is not None and len(msg) > 0:
            result_recv_data = msg.hex()
            ls = []
            for i in range(len(result_recv_data) // 2):
                ls.append(result_recv_data[i * 2] + result_recv_data[i * 2 + 1])

            if len(ls) > 0:
                status = int(ls[15])

                if status == 0:
                    rlt = (1, False)
                else:
                    rlt = (1, True)

        return rlt


    # 获取手指电机错误状态码
    def arm_error_control(self, did):
        info_list0 = []
        #头字节1
        info_list0.append('0x55')
        #头字节2
        info_list0.append('0xAA')

        info_list = []
        #字节长度
        info_list.append('0x03')
        #ID字节
        info_list.append(hex(did))
        #运行模式
        info_list.append('0x04')
        #index
        info_list.append('0x00')
        #数据段
        info_list.append('0x22')

        #帧头校验字节
        c1 = [int(i, 16) for i in info_list]
        checksum1 = hex(sum(c1))
        # print(sum(c1))
        # print(checksum1)

        if len(checksum1) == 5:
            checksum1 = checksum1[3:5]

        all_list = []
        all_list.extend(info_list0)
        all_list.extend(info_list)
        all_list.extend([checksum1])
        convert_list = []
        for str in all_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 清除手指电机错误状态码
    def arm_clearnerror_control(self, did):
        info_list0 = []
        #头字节1
        info_list0.append('0x11')
        #头字节2
        info_list0.append('0xFF')

        all_list = []
        all_list.extend(info_list0)
        convert_list = []
        for str in all_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    def arm_mul_command_control1(self, list_control):
        info_list0 = []
        #头字节1
        info_list0.append('0x60')

        info_list = []
        #循环遍历所有执行控制参数
        for info in list_control:
            #运行范围计算
            tmp_distance = list_control.get(info)
            #运行范围计算，得出16进制高位和地位
            angleControl_hex = hex(tmp_distance).replace('0x', '')

            if len(angleControl_hex) == 3:
                high_angle = angleControl_hex[1:3]
                low_angle = '0%s' % angleControl_hex[0]
            elif len(angleControl_hex) == 2:
                high_angle = angleControl_hex
                low_angle = '0'
            elif len(angleControl_hex) == 1:
                low_angle = '0'
                high_angle = '0'
            else:
                high_angle = angleControl_hex[2:4]
                low_angle = angleControl_hex[0:2]

            # print(low_angle)
            # print(high_angle)

            info_list.append(low_angle)
            info_list.append(high_angle)

        all_list = []
        all_list.extend(info_list0)
        all_list.extend(info_list)
        convert_list = []
        for str in all_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    def arm_mul_command_control2(self, list_control):
        info_list0 = []
        #头字节1
        info_list0.append('0x61')

        info_list = []
        #循环遍历所有执行控制参数
        for info in list_control:
            #运行范围计算
            tmp_distance = list_control.get(info)
            #运行范围计算，得出16进制高位和地位
            angleControl_hex = hex(tmp_distance).replace('0x', '')

            if len(angleControl_hex) == 3:
                high_angle = angleControl_hex[1:3]
                low_angle = '0%s' % angleControl_hex[0]
            elif len(angleControl_hex) == 2:
                high_angle = angleControl_hex
                low_angle = '0'
            elif len(angleControl_hex) == 1:
                low_angle = '0'
                high_angle = '0'
            else:
                high_angle = angleControl_hex[2:4]
                low_angle = angleControl_hex[0:2]

            # print(low_angle)
            # print(high_angle)

            info_list.append(low_angle)
            info_list.append(high_angle)

        all_list = []
        all_list.extend(info_list0)
        all_list.extend(info_list)
        convert_list = []
        for str in all_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)

    #多手指关节控制
    def SetARMMulData(self, list_control, isPrint=False):
        # 命令同步发送先进行左右手拆分
        left_list1 = {}
        left_list2 = {}
        right_list1 = {}
        right_list2 = {}
        for k, v in list_control:
            # 检查手指关节限位
            max_min = arm_max_min_limit.get(k)
            if max_min is None:
                print(k, '-- 输入手指关节ID无效')
                continue

            max_v = max_min[0]
            min_v = max_min[1]
            if v <= max_v and v >= min_v:
                if k == 51 or k == 52 or k == 53:
                    left_list1[k] = v
                elif k == 54 or k == 55 or k == 56:
                    left_list2[k] = v
                elif k == 61 or k == 62 or k == 63:
                    right_list1[k] = v
                elif k == 64 or k == 65 or k == 66:
                    right_list2[k] = v
            else:
                print(k, '-- 手指关节超出执行限制')
                continue

        left_list1 = dict(sorted(left_list1.items()))
        left_list2 = dict(sorted(left_list2.items()))
        right_list1 = dict(sorted(right_list1.items()))
        right_list2 = dict(sorted(right_list2.items()))

        #左手操作，左手指拆分为两段进行解析
        #分别是0x60 - 大拇指、食指、中指，0x61 - 无名指、小拇指、大拇指2
        left_run_command1 = self.arm_mul_command_control1(left_list1)
        if len(left_list1) > 0:
            left_port = port_mapping.get(51)
            left_ip = ip_mapping.get(51)
            if isPrint == True:
                print(left_list1, left_port, left_ip)
                print('左手手指批量执行命令1: %s' % left_run_command1)
            self.arm_send_command(left_ip, left_port, left_run_command1)
        time.sleep(0.001)

        left_run_command2 = self.arm_mul_command_control2(left_list2)
        if len(left_list2) > 0:
            left_port = port_mapping.get(51)
            left_ip = ip_mapping.get(51)
            if isPrint == True:
                print(left_list2, left_port, left_ip)
                print('左手手指批量执行命令2: %s' % left_run_command2)
            self.arm_send_command(left_ip, left_port, left_run_command2)
        time.sleep(0.001)

        # #右手操作，右手指拆分为两段进行解析
        # #分别是0x60 - 大拇指、食指、中指，0x61 - 无名指、小拇指、大拇指2
        right_run_command1 = self.arm_mul_command_control1(right_list1)
        if len(right_list1) > 0:
            right_port = port_mapping.get(61)
            right_ip = ip_mapping.get(61)
            if isPrint == True:
                print(right_list1, right_port, right_ip)
                print('右手手指批量执行命令1: %s' % right_run_command1)
            self.arm_send_command(right_ip, right_port, right_run_command1)
        time.sleep(0.001)

        right_run_command2 = self.arm_mul_command_control2(right_list2)
        if len(right_list2) > 0:
            right_port = port_mapping.get(61)
            right_ip = ip_mapping.get(61)
            if isPrint == True:
                print(right_list2, right_port, right_ip)
                print('右手手指批量执行命令2: %s' % right_run_command2)
            self.arm_send_command(right_ip, right_port, right_run_command2)


    # 获取单电机错误状态码
    def GetErrorStatus(self, did, isPrint=False):
        rlt = (0, False)
        try:
            conver_port = port_mapping.get(did)
            conver_id = id_mapping.get(did)
            conver_ip = ip_mapping.get(did)
            run_command = self.arm_error_control(conver_id)
            if isPrint == True:
                print('手指错误状态确认命令: %s' % run_command)
            rlt = self.arm_error_command_rlt(conver_ip, conver_port, run_command)
        except:
            pass

        return rlt


    # 清除电机错误状态码
    def CleanErrorStatus(self, did, isPrint=False):
        conver_port = port_mapping.get(did)
        conver_id = id_mapping.get(did)
        conver_ip = ip_mapping.get(did)
        run_command = self.arm_clearnerror_control(conver_id)
        if isPrint == True:
            print(conver_id, conver_ip, conver_port)
            print('手指错误状态清除命令: %s' % run_command)
        self.arm_send_command(conver_ip, conver_port, run_command)


    # 单电机运动
    def arm_set_command_control(self, did, real_angleControl):
        info_list0 = []
        #头字节1
        info_list0.append('0x55')
        #头字节2
        info_list0.append('0xAA')

        info_list = []
        #字节长度
        info_list.append('0x04')
        #ID字节
        info_list.append(hex(did))
        #运行模式
        info_list.append('0x21')
        #index
        info_list.append('0x37')
        #运行范围计算，得出16进制高位和地位
        angleControl_hex = hex(real_angleControl).replace('0x', '')

        if len(angleControl_hex) == 3:
            low_angle = angleControl_hex[1:3]
            high_angle = '0%s' % angleControl_hex[0]
        elif len(angleControl_hex) == 2:
            low_angle = angleControl_hex
            high_angle = '0'
        elif len(angleControl_hex) == 1:
            low_angle = '0'
            high_angle = '0'
        else:
            low_angle = angleControl_hex[2:4]
            high_angle = angleControl_hex[0:2]


        # print(low_angle)
        # print(high_angle)

        info_list.append(low_angle)
        info_list.append(high_angle)

        #帧头校验字节
        c1 = [int(i, 16) for i in info_list]
        checksum1 = hex(sum(c1))
        # print(sum(c1))
        # print(checksum1)

        if len(checksum1) == 5:
            checksum1 = checksum1[3:5]
            # print(checksum1)

        all_list = []
        all_list.extend(info_list0)
        all_list.extend(info_list)
        all_list.extend([checksum1])
        convert_list = []
        for str in all_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 手指电机发送记录返回值
    def arm_sendSingle_command_rlt(self, ip, port, command):
        try:
            msg = bytearray.fromhex(command)
            # print(msg)
            udp_monitor_addr1 = (ip, port)
            udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

            udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
            # print('命令发送成功...')
            # time.sleep(0.0001) # 求情等待时间
            udp_monitor_sock1.settimeout(0.01) # 请求超时时间，不设置会等待30s超时
            msg, address = udp_monitor_sock1.recvfrom(1024)
            # print(msg)
            # print(type(msg))
            try:
                if msg is not None and len(msg) > 0:
                    result_recv_data = msg.hex()
                    ls = []
                    for i in range(len(result_recv_data) // 2):
                        ls.append(result_recv_data[i * 2] + result_recv_data[i * 2 + 1])

                    if len(ls) == 0:
                        return None

                    # print(ls)
                    # print('--------------------')
                    low = ls[8]
                    high = ls[9]
                    real_controal = bin2dec(hex2bin((low + high)))

                    return real_controal
                else:
                    return None
            except:
                return None
        except:
            print('获取电机命令发送失败...')
            return None


if __name__ == '__main__':
    # 实例化手指电机类对象
    finger = Finger()
    '''
    51-56为左手手指，61-66为右手手指
    51/61:大拇指上下移动，范围上限：1700 ，范围下限：600
    56/66:大拇指左右移动，范围上限：1700 ，范围下限：150
    52/62:食指，范围上限：1800 ，范围下限：750
    53/63:中指，范围上限：1800 ，范围下限：750
    54/64:无名指，范围上限：1800 ，范围下限：750
    55/65:小拇指，范围上限：1800 ，范围下限：750
    '''
    # 1、手指电机操控命令
    # 入参1：list_control包含 12(手指个数) *（did = 电机ID、angleControl = 运行角度）
    # 返回参数：无
    list_control = []
    c1 = [61, 1200]
    c2 = [62, 1800]
    c3 = [63, 1800]
    c4 = [64, 1800]
    c5 = [65, 1800]
    c6 = [66, 600]

    c7 = [51, 1200]
    c8 = [52, 1800]
    c9 = [53, 1800]
    c10 = [54, 1800]
    c11 = [55, 1800]
    c12 = [56, 600]
    list_control.append(c1)
    list_control.append(c2)
    list_control.append(c3)
    list_control.append(c4)
    list_control.append(c5)
    list_control.append(c6)
    list_control.append(c7)
    list_control.append(c8)
    list_control.append(c9)
    list_control.append(c10)
    list_control.append(c11)
    list_control.append(c12)
    finger.SetARMMulData(list_control)


    # 2、清除手机电机错误状态
    # 入参1：did = 电机ID，
    # 当需要左手手指清除错误状态入参为51
    # 当需要右手手指清除错误状态入参为61
    # 返回参数：无
    # finger.CleanErrorStatus(51, True)
    # finger.CleanErrorStatus(61, True)




